#include "ioi2c.h"

#define SET_SDA(pi2c, val) pi2c->set_sda(val)
#define SET_SCL(pi2c, val) pi2c->set_scl(val)
#define GET_SDA(pi2c)      pi2c->get_sda()
#define GET_SCL(pi2c)      pi2c->get_scl()

#define SDA_L(pi2c) SET_SDA(pi2c, 0)
#define SDA_H(pi2c) SET_SDA(pi2c, 1)
#define SCL_L(pi2c) SET_SCL(pi2c, 0)
#define SCL_H(pi2c) SET_SCL(pi2c, 1)

#define i2c_delay(pi2c) pi2c->delay()

static void i2c_start(ioi2c_t *pi2c) {
  SCL_H(pi2c);
  SDA_H(pi2c);
  i2c_delay(pi2c);
  if (!GET_SDA(pi2c)) {
    // try to make slave to release
    SCL_L(pi2c);
    i2c_delay(pi2c);
    SCL_H(pi2c);
    i2c_delay(pi2c);
  }
  SDA_L(pi2c);
  i2c_delay(pi2c);

  SCL_L(pi2c);
  i2c_delay(pi2c);
}

static void i2c_stop(ioi2c_t *pi2c) {
  SDA_L(pi2c);
  i2c_delay(pi2c);

  SCL_H(pi2c);
  i2c_delay(pi2c);

  i2c_delay(pi2c);
  SDA_H(pi2c);
  i2c_delay(pi2c);
}

static bool i2c_readbit(ioi2c_t *pi2c) {
  bool bit = 0;
  SDA_H(pi2c);
  SCL_H(pi2c);
  i2c_delay(pi2c);
  if (GET_SDA(pi2c)) {
    bit = 1;
  }
  SCL_L(pi2c);
  i2c_delay(pi2c);
  return bit;
}

static uint8_t i2c_readbyte(ioi2c_t *pi2c) {
  uint16_t byte = 1;

  do {
    byte <<= 1;
    byte |= i2c_readbit(pi2c);
  } while (!(byte & (1 << 8)));
  return byte;
}

static void i2c_writebit(ioi2c_t *pi2c, bool bit) {
  SET_SDA(pi2c, bit);
  SCL_H(pi2c);
  i2c_delay(pi2c);
  SCL_L(pi2c);
  i2c_delay(pi2c);
}

static bool i2c_writebyte(ioi2c_t *pi2c, uint8_t data) {
  uint8_t mask = 1 << 7;
  do {
    i2c_writebit(pi2c, data & mask);
  } while (mask >>= 1);

  return !i2c_readbit(pi2c);
}

static bool i2c_send_address(ioi2c_t *pi2c, uint8_t addr, uint8_t retries) {
  bool retval = false;

  i2c_start(pi2c);
  retval = i2c_writebyte(pi2c, addr);

  return retval;
}

bool i2c_readbytes(ioi2c_t *pi2c, uint8_t address, uint8_t *buf, uint16_t len) {
  if (true != i2c_send_address(pi2c, address | 0x01, 2)) {
    goto error_out;
  }
  while (len > 0) {
    --len;
    *buf++ = i2c_readbyte(pi2c);
    i2c_writebit(pi2c, len == 0);
  }
  i2c_stop(pi2c);

  return true;
error_out:
  i2c_stop(pi2c);

  return false;
}

bool i2c_writebytes(ioi2c_t *pi2c, uint8_t address, uint8_t *buf, uint16_t len) {
  if (true != i2c_send_address(pi2c, address & 0xFE, 2)) {
    goto error_out;
  }
  while (len > 0) {
    --len;
    if (true != i2c_writebyte(pi2c, *buf++)) {
      goto error_out;
    }
  }
  i2c_stop(pi2c);

  return true;
error_out:
  i2c_stop(pi2c);

  return false;
}

bool i2c_writeoffset(ioi2c_t *pi2c, uint8_t address, uint8_t *buf, uint16_t len) {
  if (true != i2c_send_address(pi2c, address & 0xFE, 2)) {
    goto error_out;
  }
  while (len > 0) {
    --len;
    if (true != i2c_writebyte(pi2c, *buf++)) {
      goto error_out;
    }
  }
  i2c_delay(pi2c);
  return true;
error_out:
  i2c_stop(pi2c);
  return false;
}
